#include "loadkinect.h"

LoadKinect::LoadKinect()
{
}
void LoadKinect::Load(QString nombre)
{
    Archivo=nombre;
}
unsigned LoadKinect::getFrameID()
{
   QMutexLocker locker(&mutex);
   return frameid;
}
QImage LoadKinect::getDepth()
{
   QMutexLocker locker(&mutex);
   return imageDepth;
}
QImage LoadKinect::getCamera()
{
   QMutexLocker locker(&mutex);
   return imageCamera;
}
void LoadKinect::start(QString nom)
{
    t_requeststop=false;
    //status=Kinect::Idle;
    Load(nom);
    QThread::start();
}

void LoadKinect::stop()
{
   t_requeststop=true;
   wait();
}
bool LoadKinect::isRunning()
{
   if(QThread::isRunning())
      return true;
   return false;

}

bool LoadKinect::isStopped()
{
   if(QThread::isRunning())
      return false;
   return true;

}

void LoadKinect::run()
{
    bool ok = initialize();
    if(!ok)
    {
       mutex.lock();
       //status = Kinect::ErrorStop;
       //emit statusNotification(status);
       mutex.unlock();
       return;
    }
    frameid=0;
    while(!t_requeststop)
    {
       XnStatus status = g_Context.WaitAndUpdateAll();
       XnStatus status2 = g_Context2.WaitAndUpdateAll();
       mutex.lock();
       xn::DepthMetaData depthMD;
       g_DepthGenerator.GetMetaData(depthMD);
       frameid++;
       imageCamera = createCameraImage();
       imageDepth = createDepthImage();
       emit dataNotification();
       mutex.unlock();
    }
    g_Context.Shutdown();
}
QImage LoadKinect::createDepthImage()
{
   // Here must mutex / run also access the data
   xn::SceneMetaData smd;
   xn::DepthMetaData dmd;
   g_DepthGenerator.GetMetaData(dmd);
   XnUInt16 g_nXRes = dmd.XRes();
   XnUInt16 g_nYRes = dmd.YRes();
   QImage image(g_nXRes,g_nYRes,QImage::Format_RGB32);
   const XnDepthPixel* pDepth = dmd.Data();
   const XnLabel* pLabels = smd.Data();
   for (unsigned nY=0; nY<g_nYRes; nY++)
   {
      uchar *imageptr = image.scanLine(nY);
      for (unsigned nX=0; nX < g_nXRes; nX++)
      {
         unsigned depth = *pDepth;
         unsigned label = *pLabels;
         unsigned maxdist=5000;
         if(depth>maxdist) depth=maxdist;
         if(depth)
         {
            depth = (maxdist-depth)*255/maxdist;
         }
            imageptr[0] = 0;
            imageptr[1] = depth;
            imageptr[2] = 0;
            imageptr[3] = 0xff;
         pDepth++;
         imageptr+=4;
         pLabels++;
      }
   }
   return image;
}
QImage LoadKinect::createCameraImage()
{
   // Here must mutex / run also access the data
   xn::DepthMetaData dmd;
   xn::ImageMetaData imd;
   g_DepthGenerator.GetMetaData(dmd);
   g_ImageGenerator.GetMetaData(imd);
   XnUInt16 g_nXRes = dmd.XRes();
   XnUInt16 g_nYRes = dmd.YRes();
   QImage image(g_nXRes,g_nYRes,QImage::Format_RGB32);
   const XnUInt8 *idata = imd.Data();
   for (unsigned nY=0; nY<g_nYRes; nY++)
   {
      uchar *imageptr = image.scanLine(nY);
      for (unsigned nX=0; nX < g_nXRes; nX++)
      {
         imageptr[0] = idata[2];
         imageptr[1] = idata[1];
         imageptr[2] = idata[0];
         imageptr[3] = 0xff;
         imageptr+=4;
         idata+=3;
      }
   }
   return image;
}
bool LoadKinect::initialize()
{
    XnStatus nRetVal = XN_STATUS_OK;
    //nRetVal = g_Context.OpenFileRecording(Archivo.toUtf8().constData());
    // Initialize the context
    nRetVal = g_Context.Init();
    nRetVal = g_Context2.Init();

    nRetVal = g_Context.OpenFileRecording(Archivo.toUtf8().constData());
    nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
    if(nRetVal!=XN_STATUS_OK)
    {
        return false;
    }
    nRetVal = g_Context2.OpenFileRecording("C:\\Users\\javier\\VisionRobot1\\release\\rataVGA.oni");
    nRetVal= g_Context2.FindExistingNode(XN_NODE_TYPE_IMAGE,g_ImageGenerator);
    if(nRetVal!=XN_STATUS_OK)
    {
        return false;
    }
    return true;
}
